#include "handLocaliser.hpp"
#include "hiDependenciesForTestFramework.h" //to enable visualisation for testing

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>


#include <iostream>


//	
//	Mat hsvFullSize;
//
//	FaceDetector* 

using namespace cv;
	
HandLocaliser::HandLocaliser() : faceDetector( new FaceDetector(1.0) ),
probTrans( Ptr< AbstractProbabilityTransformer >( new PerPixelProbabilityTransformer( Ptr< AbstractFilter >( new OpenFilter(1) ) ) ) ){}


HandLocaliser::~HandLocaliser() {
	
	delete faceDetector;
	
}
	

//FIXNOW incorporate a confidence measure in localisation?
//i.e. to make it possible that a hand is not found

Rect HandLocaliser::localiseHand( const Mat& image ) {
	
	// -- Finding face in image -- //
	
	Mat hsvFullSize;

	cvtColor(image, hsvFullSize, CV_BGR2HSV);

	vector<Rect>* faces = faceDetector->detectFace(image);

	if ( faces->empty() ) {
		
		CV_Error( HI_ERR_NOFACEFOUND, "No face was found by faceDetector in handLocaliser" ); //throws exception
	}

	Rect faceRect = faces->front();

	Rect narrowFaceRect = Rect( faceRect.x+faceRect.width/6, faceRect.y, 2*faceRect.width/3, faceRect.height);
	
	//The following can be enabled for testing
#ifdef HI_TEST_VISUALISATION
	// -- visualise face detection -- //
	{
		Mat visualisation_image;
		
		image.copyTo( visualisation_image );
		
		//Draw full face rect 
		rectangle( visualisation_image, Point( faceRect.x, faceRect.y ), //WAS bug : cvRectangle instead
				  Point( faceRect.x+faceRect.width, faceRect.y + faceRect.height ),
				  Scalar::all(0),
				  2);
		
		//Draw narrow face rect
		rectangle( visualisation_image, Point( narrowFaceRect.x, narrowFaceRect.y ), //WAS bug : cvRectangle instead
				  Point( narrowFaceRect.x+narrowFaceRect.width, narrowFaceRect.y + narrowFaceRect.height ),
				  Scalar::all(100),
				  2);
		namedWindow( "Face detection in handLocaliser", 1 );
		imshow( "Face detection in handLocaliser", visualisation_image );
		
		waitKey();
	}
#endif
	

	Mat hsvFace( hsvFullSize, narrowFaceRect );

	// -- Computing histogram and setting histogram for backprojection -- //

	probTrans->calcAndSetHist( hsvFace );

	// -- Finding the estimated torso region -- //

	//Torso start set to 1 face width to the left of face start,
	//or if that's outside the frame, just 0.
	//NOTE: Not using narrowFaceRect sizes!
	int torsoApproxStartX = ( faceRect.x > faceRect.width ) ? faceRect.x - faceRect.width : 0;

	//Torso should be so that the torso reaches face width out from face to either side, or maximum possible out to either side,
	//if that is too much to fit inside frame.
	
	int desiredWidth = 2*faceRect.width + faceRect.x - torsoApproxStartX;
	
	int torsoApproxWidth = ( image.cols > desiredWidth + torsoApproxStartX ) ? desiredWidth : image.cols - torsoApproxStartX;
	
	//FIX : throw exception if face is too near bottom?
	int neckTop = (image.rows > faceRect.y + 3*faceRect.width/2) ? faceRect.y + 3*faceRect.width/2 : faceRect.y;

	Rect torsoApproxRect( torsoApproxStartX, neckTop,
					 torsoApproxWidth, image.rows - neckTop);
	
	Mat hsvTorso( hsvFullSize, torsoApproxRect);
	
	// -- Calculating back projection: Probability of hand in torso region -- //
	
	Mat backProj = Mat::zeros(hsvTorso.cols, hsvTorso.rows, CV_8UC1);
	
	probTrans->getBackProjection( hsvTorso, backProj);

	//		rectangle( src, Point( torsoApproxStartX, neckTop ), //WAS bug : cvRectangle instead
	//			  Point( torsoApproxStartX + torsoApproxWidth, src.rows ),
	//			  Scalar::all(0),
	//			  2);

	// -- Localise hand -- //
	//FIX: This is basic. Use something cleverer. E.g. connected components + pick largest component
	
	Point handCenterGuess = initialLocationGuess( backProj );
	
	//FIX: handle edge cases better! Literally, handle it better when hand is near edge of image.
	Rect handRect = Rect( handCenterGuess.x - faceRect.width/2 , handCenterGuess.y - faceRect.height/2 ,
						 faceRect.width , faceRect.height );
	

	if ( handRect.x < 0) handRect.x = 0;
	if ( handRect.y < 0) handRect.y = 0;
	if ( backProj.cols < handRect.x + handRect.width) handRect.width = backProj.cols - handRect.x;
	if ( backProj.rows < handRect.y + handRect.width) handRect.height = backProj.rows - handRect.y;

	
#ifdef HI_TEST_VISUALISATION
	//Visualise initial hand location guess
	{
		Mat visualisation_image;
		
		backProj.copyTo( visualisation_image );
		 
		rectangle( visualisation_image, Point( handRect.x, handRect.y ), //WAS bug : cvRectangle instead
				  Point( handRect.x+handRect.width, handRect.y + handRect.height ),
				  Scalar::all(100),
				  2);
		
		namedWindow( "Initial hand location guess in handLocaliser", 1 );
		imshow( "Initial hand location guess in handLocaliser", visualisation_image );
		
		waitKey();
	}
#endif
	
	meanShift( backProj, handRect, TermCriteria( TermCriteria::MAX_ITER + TermCriteria::EPS , 60, 0.001) );
	//handRect is moved
	
#ifdef HI_TEST_VISUALISATION
	//Visualise hand location guess after meanshift
	{
		Mat visualisation_image;
		
		backProj.copyTo( visualisation_image );
		
		rectangle( visualisation_image, Point( handRect.x, handRect.y ), //WAS bug : cvRectangle instead
				  Point( handRect.x+handRect.width, handRect.y + handRect.height ),
				  Scalar::all(200),
				  2);
		
		namedWindow( "Initial hand location guess in handLocaliser", 1 );
		imshow( "Initial hand location guess in handLocaliser", visualisation_image );
		
		waitKey();
	}
#endif

	
	Rect handRectInFullImage( handRect.x+handRect.width/2-faceRect.width/2 + torsoApproxStartX, //+torsoApproxStartX to move to frame of full image (x-coord)
								  handRect.y+handRect.height/2-faceRect.height/2 + neckTop, //+neckTop to move to frame of full image y-coord)
								  faceRect.width, faceRect.height);
	
	//WARNING: this rectangle is NOT guaranteed to lie within the image. It should be clipped as necessary when used.
								  
	return handRectInFullImage;
}

cv::Ptr< AbstractProbabilityTransformer > HandLocaliser::getProbabilityTransformer() {
	return probTrans;	
}



// based on code from camshift.cpp
//TODO : not static?
Point HandLocaliser::initialLocationGuess( const Mat& probImage ) {
	
	//FIX investigate correct Matrix conversion
	
	CvMat _probImage = probImage;
	
	CvMoments moments;
	CvMat  stub, *mat = (CvMat*) &_probImage;
	//stub is used as a tmp buffer just for the copying of imgProb header info to mat
	
	moments.m00 = moments.m10 = moments.m01 = 0;
	
	mat = cvGetMat( mat, &stub );

	if( CV_MAT_CN( mat->type ) > 1 )
		CV_Error(CV_BadNumChannels, "Too many channels in input image to initialLocationGuess function in handLocaliser. Expected 1 channel only.");
	
	int cx, cy;
	double inv_m00;
	
	cvMoments( mat, &moments );
	
	inv_m00 = moments.inv_sqrt_m00*moments.inv_sqrt_m00;
	cx = cvRound( moments.m10 * inv_m00);
	cy = cvRound( moments.m01 * inv_m00);
	
	return Point(cx,cy);
}








// -- NEW -- //
Rect HandLocaliser::localiseFace( const Mat& image ) {
	
	// -- Finding face in image -- //
	
	Mat hsvFullSize;
	
	cvtColor(image, hsvFullSize, CV_BGR2HSV);
	
	vector<Rect>* faces = faceDetector->detectFace(image);
	
	if ( faces->empty() ) {
		
		CV_Error( HI_ERR_NOFACEFOUND, "No face was found by faceDetector in handLocaliser" ); //throws exception
	}
	
	Rect faceRect = faces->front();
	
	Rect narrowFaceRect = Rect( faceRect.x+faceRect.width/6, faceRect.y, 2*faceRect.width/3, faceRect.height);
	
	//The following can be enabled for testing
#ifdef HI_TEST_VISUALISATION
	// -- visualise face detection -- //
	{
		Mat visualisation_image;
		
		image.copyTo( visualisation_image );
		
		//Draw full face rect 
		rectangle( visualisation_image, Point( faceRect.x, faceRect.y ), //WAS bug : cvRectangle instead
				  Point( faceRect.x+faceRect.width, faceRect.y + faceRect.height ),
				  Scalar::all(0),
				  2);
		
		//Draw narrow face rect
		rectangle( visualisation_image, Point( narrowFaceRect.x, narrowFaceRect.y ), //WAS bug : cvRectangle instead
				  Point( narrowFaceRect.x+narrowFaceRect.width, narrowFaceRect.y + narrowFaceRect.height ),
				  Scalar::all(100),
				  2);
		namedWindow( "Face detection in handLocaliser", 1 );
		imshow( "Face detection in handLocaliser", visualisation_image );
		
		waitKey();
	}
#endif
	
	
	Mat hsvFace( hsvFullSize, narrowFaceRect );
	
	// -- Computing histogram and setting histogram for backprojection -- //
	
	probTrans->calcAndSetHist( hsvFace );

	return faceRect;
}





